% 一个避障的测试程序，有可视化部分
clear; 
clc;

% 初始化避障系统
ship_length = 10.0; % 米
ship_width = 3.0;   % 米
min_turn_radius = 20.0; % 米
avoidance = ObstacleAvoidance(ship_length, ship_width, min_turn_radius);

% 模拟雷达数据 (Nx3数组: x,y,z)
point_cloud = [
    30,  5, 0.5;   % 右前方障碍物
    40, -3, 0.2;   % 正前方稍偏左障碍物
    25, -10, 0.8;  % 左前方障碍物
    -10, 15, 0.1;  % 右后方障碍物(应被忽略)
    50, 0, 1.5;    % 正前方远距离障碍物
    35, 8, 0.3     % 右前方障碍物
];

current_speed = 3.0; % m/s
current_heading = deg2rad(45); % 45度

% 计算目标点
target_point = avoidance.avoid_obstacles(point_cloud, current_speed, current_heading);

%% 可视化
figure('Name', '避障算法可视化', 'NumberTitle', 'off');
hold on; grid on; axis equal;
xlabel('X (m)'); ylabel('Y (m)');
title('船舶避障路径规划');

% 绘制船体位置和方向
draw_ship(0, 0, current_heading, ship_length, ship_width);

% 绘制雷达点云(障碍物)
obstacles_2d = avoidance.process_radar_data(point_cloud);
scatter(obstacles_2d(:,1), obstacles_2d(:,2), 'r', 'filled');
text(obstacles_2d(:,1), obstacles_2d(:,2)+2, num2str((1:size(obstacles_2d,1))'), 'Color', 'red');

% 绘制安全扇形区域
draw_safety_sectors(avoidance, obstacles_2d, current_speed, current_heading);

% 绘制目标点
scatter(target_point(1), target_point(2), 'g', 'filled', 'Marker', 'd');
plot([0, target_point(1)], [0, target_point(2)], 'g--');
text(target_point(1), target_point(2)+3, '目标点', 'Color', 'green');

% 绘制前瞻距离圆
theta = linspace(0, 2*pi, 100);
adaptive_distance = avoidance.lookahead_distance * (1 + current_speed / 5.0);
plot(adaptive_distance * cos(theta), adaptive_distance * sin(theta), 'b:');

legend('船体', '障碍物', '目标点', '路径', '前瞻距离');

%% 辅助绘图函数
function draw_ship(x, y, heading, length, width)
    % 绘制船体
    rotation_matrix = [
        cos(heading), -sin(heading);
        sin(heading),  cos(heading)
    ];
    
    % 船体轮廓点
    hull_points = [
        length/2,  width/2;
        length/2, -width/2;
        -length/2, -width/2;
        -length/2,  width/2
    ];
    
    % 旋转和平移船体
    rotated_hull = hull_points * rotation_matrix';
    rotated_hull(:,1) = rotated_hull(:,1) + x;
    rotated_hull(:,2) = rotated_hull(:,2) + y;
    
    % 绘制船体
    fill(rotated_hull(:,1), rotated_hull(:,2), 'b', 'FaceAlpha', 0.3);
    
    % 绘制船头方向指示线
    head_x = x + length/2 * cos(heading);
    head_y = y + length/2 * sin(heading);
    plot([x, head_x], [y, head_y], 'b', 'LineWidth', 2);
end

function draw_safety_sectors(avoidance, obstacles, current_speed, current_heading)
    % 计算安全方向
    risks = avoidance.calculate_risk(obstacles, current_speed, current_heading);
    sector_safety = avoidance.find_safe_directions(obstacles, risks, current_heading);
    
    num_sectors = length(sector_safety);
    sector_angles = linspace(0, 2*pi, num_sectors+1);
    adaptive_distance = avoidance.lookahead_distance * (1 + current_speed / 5.0);
    
    % 绘制扇形区域
    for i = 1:num_sectors
        angle1 = sector_angles(i) + current_heading;
        angle2 = sector_angles(i+1) + current_heading;
        
        % 扇形边缘点
        t = linspace(angle1, angle2, 20);
        x = [0, adaptive_distance * cos(t), 0];
        y = [0, adaptive_distance * sin(t), 0];
        
        % 根据安全性设置颜色
        safety = sector_safety(i);
        if safety > 0.7
            color = [0, 1, 0, 0.2]; % 绿色(安全)
        elseif safety > 0.4
            color = [1, 1, 0, 0.2]; % 黄色(中等)
        else
            color = [1, 0, 0, 0.2]; % 红色(危险)
        end
        
        patch(x, y, color(1:3), 'EdgeColor', 'none', 'FaceAlpha', color(4));
    end
end